
// Copyright (C) 2009-2011 Luca Piccioni
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//  
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//  
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.

using System;
using System.Globalization;
using System.Xml;
using System.Xml.Schema;
using System.Xml.Serialization;

namespace Derm.Render
{
	/// <summary>
	/// Quaternion.
	/// </summary>
	/// <remarks>
	/// <para>
	/// Quaternion are used to represents a rotation in a three-dimensional space.
	/// </para>
	/// <para>
	/// Theory links:
	/// - http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
	/// - http://en.wikipedia.org/wiki/Rotation_representation
	/// </para>
	/// </remarks>
	public struct Quaternion : IXmlSerializable
	{
		#region Constructors

		/// <summary>
		/// Construct a Quaternion from quaternion components.
		/// </summary>
		/// <param name="q1">
		/// A <see cref="Single"/> that specify the quaternion component <i>q1</i>.
		/// </param>
		/// <param name="q2">
		/// A <see cref="Single"/> that specify the quaternion component <i>q2</i>.
		/// </param>
		/// <param name="q3">
		/// A <see cref="Single"/> that specify the quaternion component <i>q3</i>.
		/// </param>
		/// <param name="q4">
		/// A <see cref="Single"/> that specify the quaternion component <i>q4</i> (scalar component).
		/// </param>
		public Quaternion(float q1, float q2, float q3, float q4)
		{
			// Set the default rotation axis
			mDefaultVector = Vertex3d.UnitY;
			// Setup quaternion components
			mVector.x = q1;
			mVector.y = q2;
			mVector.z = q3;
			mCosAngle = q4;
			// Derive default rotation vector (due quaternion definition)
			mDefaultVector = (Vertex3d)RotationVector;
		}

		/// <summary>
		/// Quaternion constructor from euler rotation axis and rotation angle.
		/// </summary>
		/// <param name="rVector">
		/// A <see cref="Vertex3f"/> representing the rotation axis.
		/// </param>
		/// <param name="rAngle">
		/// A <see cref="System.Single"/> representing the rotation angle (in radians).
		/// </param>
		/// <remarks>
		/// This constructor is the base implementation for each other constructor.
		/// </remarks>
		public Quaternion(Vertex3f rVector, float rAngle)
		{
			// Set the default rotation axis
			mDefaultVector = (Vertex3d)rVector;
			// Make compiler happy
			mVector = new Vertex3d();
			mCosAngle = 0.0f;

			// Set quaternion
			SetEuler(rVector, rAngle);
		}

		#endregion

		#region Structure

		/// <summary>
		/// The default rotation axis used by this quaternion when it is an identity.
		/// </summary>
		private readonly Vertex3d mDefaultVector;

		/// <summary>
		/// Quaternion vector values.
		/// </summary>
		/// <remarks>
		/// If the quaternion is defined as a vector {q1, q2, q3, q4}, this field corresponds to the
		/// vector component {q1, q2, q3}. This is not actually a vector!
		/// </remarks>
		private Vertex3d mVector;

		/// <summary>
		/// Quaternion "scalar" component.
		/// </summary>
		/// <remarks>
		/// If the quaternion is defined as a vector {q1, q2, q3, q4}, this field corresponds to the
		/// scalar component q4. This is not actually an angle!
		/// </remarks>
		private double mCosAngle;

		#endregion

		#region Euler Definitions

		/// <summary>
		/// Rotation unit vector.
		/// </summary>
		public Vertex3f RotationVector
		{
			get
			{
				if (mVector.Module() > Single.Epsilon)
					return (Vertex3f)(mVector / mVector.Module());
				else
					return (Vertex3f)(mDefaultVector);
			}
			set { SetEuler(value, RotationAngle); }
		}

		/// <summary>
		/// Quaternion rotation angle (in degrees).
		/// </summary>
		public float RotationAngle
		{
			get { return (float)(2.0 * Math.Acos(mCosAngle) * RadToDeg); }
			set { SetEuler(RotationVector, value); }
		}

		/// <summary>
		/// Set quaternion using rotation axis and rotation angle.
		/// </summary>
		/// <param name="rVector">
		/// A <see cref="Vertex3f"/> representing the rotation axis. It will be normalized.
		/// </param>
		/// <param name="rAngle">
		/// A <see cref="System.Single"/> representing the rotation angle (in degrees).
		/// </param>
		/// <remarks>
		/// This quaternion will result normalized.
		/// </remarks>
		private void SetEuler(Vertex3f rVector, float rAngle)
		{
			double qAngle = (rAngle * DegToRad) / 2.0f;
			double qAngleSin = Math.Sin(qAngle);

			rVector.Normalize();

			mVector.x = (float)(qAngleSin * rVector.x);
			mVector.y = (float)(qAngleSin * rVector.y);
			mVector.z = (float)(qAngleSin * rVector.z);
			mCosAngle = Math.Cos(qAngle);
		}

		/// <summary>
		/// Constant used for converting radians to degrees by multiplication.
		/// </summary>
		private const double RadToDeg = 180.0 / Math.PI;

		/// <summary>
		/// Constant used for converting degress to radians by multiplication.
		/// </summary>
		private const double DegToRad = Math.PI / 180.0;

		#endregion

		#region Quaternion Properties

		/// <summary>
		/// Identity quaternion (no rotation).
		/// </summary>
		public static readonly Quaternion Identity = new Quaternion(0.0f, 0.0f, 0.0f, 1.0f);

		/// <summary>
		/// Quaternion <i>q1</i> component.
		/// </summary>
		public float X { get { return ((float)mVector.x); } set { mVector.x = value; } }

		/// <summary>
		/// Quaternion <i>q2</i> component.
		/// </summary>
		public float Y { get { return ((float)mVector.y); } set { mVector.y = value; } }

		/// <summary>
		/// Quaternion <i>q3</i> component.
		/// </summary>
		public float Z { get { return ((float)mVector.z); } set { mVector.z = value; } }

		/// <summary>
		/// Quaternion <i>q4</i> component.
		/// </summary>
		public float W { get { return ((float)mCosAngle); } set { mCosAngle = value; } }

		/// <summary>
		/// Compute this Quaternion magnitude.
		/// </summary>
		public double Magnitude
		{
			get {
				double x2 = mVector.x * mVector.x;
				double y2 = mVector.y * mVector.y;
				double z2 = mVector.z * mVector.z;
				double w2 = mCosAngle * mCosAngle;

				return (Math.Sqrt(x2 + y2 + z2 + w2));
			}
		}

		/// <summary>
		/// Determine whether this quaternion is identity (no rotation).
		/// </summary>
		public bool IsIdentity
		{
			get
			{
				if (Math.Abs(mVector.Module()) >= Single.Epsilon)
					return (false);
				if (Math.Abs(mCosAngle - 1.0f) >= Single.Epsilon)
					return (false);

				return (true);
			}
		}

		/// <summary>
		/// Determine whether this quaternion is normalized.
		/// </summary>
		public bool IsNormalized
		{
			get { return (Math.Abs(Magnitude - 1.0) < Single.Epsilon); }
		}

		#endregion

		#region Quaternion Operations

		/// <summary>
		/// Normalize this Quaternion.
		/// </summary>
		public void Normalize()
		{
			double magnitude = Magnitude;

			if (magnitude >= Single.Epsilon) {
				double scale = 1.0 / Magnitude;

				mVector *= scale;
				mCosAngle *= scale;	
			} else
				throw new InvalidOperationException("zero magnitude quaternion");
		}

		/// <summary>
		/// Conjugate this Quaternion.
		/// </summary>
		public Quaternion Conjugate
		{
			get
			{
				Quaternion conjugate = this;

				conjugate.mVector = -conjugate.mVector;

				return (conjugate);
			}
		}

		#endregion

		#region Arithmetich Operators

		/// <summary>
		/// Quaternion multiplication (concatenate rotations).
		/// </summary>
		/// <param name="q1">
		/// A <see cref="Quaternion"/> representing the left multiplication operand. This rotation is the "starting" state.
		/// </param>
		/// <param name="q2">
		/// A <see cref="Quaternion"/> representing the right multiplication operand. This rotation is applied to <paramref name="q1"/>.
		/// </param>
		/// <returns>
		/// It returns the multiplication between two quaternions.
		/// </returns>
		public static Quaternion operator *(Quaternion q1, Quaternion q2)
		{
			Quaternion q = new Quaternion();
			double x1 = q1.mVector.x, y1 = q1.mVector.y, z1 = q1.mVector.z, w1 = q1.mCosAngle;
			double x2 = q2.mVector.x, y2 = q2.mVector.y, z2 = q2.mVector.z, w2 = q2.mCosAngle;

			q.mCosAngle = w2*w1 - x2*x1 - y2*y1 - z2*z1;
			q.mVector.x = w2*x1 + x2*w1 - y2*z1 + z2*y1;
			q.mVector.y = w2*y1 + x2*z1 + y2*w1 - z2*x1;
			q.mVector.z = w2*z1 - x2*y1 + y2*x1 + z2*w1;

			return (q);
		}

		#endregion

		#region Cast Operators

		/// <summary>
		/// 
		/// </summary>
		/// <param name="q">
		/// A <see cref="Quaternion"/> to be casted to <see cref="Matrix4x4"/>.
		/// </param>
		/// <returns>
		/// It returns <see cref="Matrix3x3"/> representing the equivalent rotation matrix.
		/// </returns>
		public static explicit operator Matrix3x3(Quaternion q)
		{
			Matrix3x3 dcm = new Matrix3x3();		// Direction cosine matrix
			double q1 = q.mVector.x, q2 = q.mVector.y, q3 = q.mVector.z, q4 = q.mCosAngle;
			double q1__2 = q1*q1, q2__2 = q2*q2, q3__2 = q3*q3;
			
			dcm[0,0] = (float)(1.0f - 2.0f * (q2__2 + q3__2));
			dcm[1,0] = (float)(		  2.0f * (q1*q2 - q3*q4));
			dcm[2,0] = (float)(		  2.0f * (q1*q3 + q2*q4));

			dcm[0,1] = (float)(		  2.0f * (q1*q2 + q3*q4));
			dcm[1,1] = (float)(1.0f - 2.0f * (q1__2 + q3__2));
			dcm[2,1] = (float)(		  2.0f * (q2*q3 - q1*q4));

			dcm[0,2] = (float)(		  2.0f * (q1*q3 - q2*q4));
			dcm[1,2] = (float)(		  2.0f * (q1*q4 + q2*q3));
			dcm[2,2] = (float)( 1.0f - 2.0f * (q1__2 + q2__2));

			return (dcm);
		}

		/// <summary>
		/// 
		/// </summary>
		/// <param name="q">
		/// A <see cref="Quaternion"/> to be casted to <see cref="Matrix4x4"/>.
		/// </param>
		/// <returns>
		/// It returns <see cref="ModelMatrix"/> representing the equivalent rotation matrix.
		/// </returns>
		public static explicit operator Matrix4x4(Quaternion q)
		{
			Matrix4x4 dcm = new Matrix4x4();		// Direction cosine matrix
			double q1 = q.mVector.x, q2 = q.mVector.y, q3 = q.mVector.z, q4 = q.mCosAngle;
			double q1__2 = q1*q1, q2__2 = q2*q2, q3__2 = q3*q3;
			
			dcm[0,0] = (float)(1.0f - 2.0f * (q2__2 + q3__2));
			dcm[1,0] = (float)(		  2.0f * (q1*q2 - q3*q4));
			dcm[2,0] = (float)(		  2.0f * (q1*q3 + q2*q4));
			dcm[3,0] = 0.0f;

			dcm[0,1] = (float)(		  2.0f * (q1*q2 + q3*q4));
			dcm[1,1] = (float)(1.0f - 2.0f * (q1__2 + q3__2));
			dcm[2,1] = (float)(		  2.0f * (q2*q3 - q1*q4));
			dcm[3,1] = 0.0f;

			dcm[0,2] = (float)(		  2.0f * (q1*q3 - q2*q4));
			dcm[1,2] = (float)(		  2.0f * (q1*q4 + q2*q3));
			dcm[2,2] = (float)( 1.0f - 2.0f * (q1__2 + q2__2));
			dcm[3,2] = 0.0f;

			dcm[0,3] = 0.0f;
			dcm[1,3] = 0.0f;
			dcm[2,3] = 0.0f;
			dcm[3,3] = 1.0f;

			return (dcm);
		}

		#endregion

		#region IXmlSerializable Implementation

		/// <summary>
		/// This property is reserved, apply the System.Xml.Serialization.XmlSchemaProviderAttribute to the class instead.
		/// </summary>
		/// <returns>
		/// An System.Xml.Schema.XmlSchema that describes the XML representation of the object that is produced by the
		/// System.Xml.Serialization.IXmlSerializable.WriteXml(System.Xml.XmlWriter) method and consumed by the
		/// System.Xml.Serialization.IXmlSerializable.ReadXml(System.Xml.XmlReader) method.
		/// </returns>
		public XmlSchema GetSchema() { return (null); }

		/// <summary>
		/// Generates an object from its XML representation.
		/// </summary>
		/// <param name="reader">
		/// The System.Xml.XmlReader stream from which the object is deserialized.
		/// </param>
		public void ReadXml(XmlReader reader)
		{
			Vertex3f eulerVector = new Vertex3f();
			float eulerAngle = 0.0f;

			string quatComponent;

			// Attribute: euler (normalized) vector
			if ((quatComponent = reader["X"]) != null)
				eulerVector.x = Single.Parse(quatComponent, NumberFormatInfo.InvariantInfo);
			if ((quatComponent = reader["Y"]) != null)
				eulerVector.y = Single.Parse(quatComponent, NumberFormatInfo.InvariantInfo);
			if ((quatComponent = reader["Z"]) != null)
				eulerVector.z = Single.Parse(quatComponent, NumberFormatInfo.InvariantInfo);
			// Attribute: euler angle
			if ((quatComponent = reader["Angle"]) != null)
				eulerAngle = Single.Parse(quatComponent, NumberFormatInfo.InvariantInfo);

			// Setup quaternion
			SetEuler(eulerVector, eulerAngle);
		}

		/// <summary>
		/// Converts an object into its XML representation.
		/// </summary>
		/// <param name="writer">
		/// The System.Xml.XmlWriter stream to which the object is serialized.
		/// </param>
		public void WriteXml(XmlWriter writer)
		{
			Vertex3f eulerVector = RotationVector;
			float eulerAngle = RotationAngle;

			// Attribute: euler (normalized) vector
			writer.WriteAttributeString("X", eulerVector.x.ToString(NumberFormatInfo.InvariantInfo));
			writer.WriteAttributeString("Y", eulerVector.y.ToString(NumberFormatInfo.InvariantInfo));
			writer.WriteAttributeString("Z", eulerVector.z.ToString(NumberFormatInfo.InvariantInfo));
			// Attribute: euler angle
			writer.WriteAttributeString("Angle", eulerAngle.ToString(NumberFormatInfo.InvariantInfo));

			// Example of output:
			//
			// <Quaternion X="0.0" Y="0.0" Z="0.0" Angle="0.0" />
		}

		#endregion

		#region Object Overrides

		/// <summary>
		/// Stringify this Quaternion.
		/// </summary>
		/// <returns>
		/// Returns a <see cref="System.String"/> that represents this Quaternion.
		/// </returns>
		public override string ToString()
		{
			return ("Axis: "+RotationVector+" Angle: "+RotationAngle);
		}

		#endregion
	}
}